#ifndef RSIM_CYLINDER_H
#define RSIM_CYLINDER_H

/* -------------------------------------------------------------------------- *
 * File: Cylinder.h                                                           *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Joint.h"
#include "Utils.h"

namespace RSIM{
        
class ABAData;
class EOMData;

/** 
<p>1. Cylinder joint is between the z axis of frames M and F of the connnecting bodies.</p>
<p>2. Q[0] = rotation degree of freedom and Q[1] is the translational degree of freedom. </p>
Z axis of frames F and M always remains aligned.
*/

class Cylinder:public Joint{
        private:
                /** Create a ball joint with M and F coincident in default configuration */
                Cylinder(const int& NodeIndex);
        public:
                /** Create a new joint */
                static Cylinder* New(const int& NodeIndex);
                
                /** Print the joint type */
                void printJointType() const;
                
                void getHColumn(const int& uindex, 
                                double *hptr)const;
                
                const bool areQuaternionsInUse()const{return false;}
                
                int getDOF()const{return 2;}
                
                void setNQ(int *q)const{q[0]=2;}
                
                void setNU(int *u)const{u[0]=2;}
                
                /** fills *q with default values of q. */
                void setDefaultQ(double *q)const;
                
                /** fills *u with default values of q*/
                void setDefaultU(double *u)const;		
                
                /** Update joint Transform */
                void updJointTransform(const double *q,
                                const RigidBodyNode *Parent,
                                const int& pChildID,
                                double *betak);
                                
                void updBetaK(	const double *q,
                                const RigidBodyNode *Parent,
                                const int& pChildID,
                                double *betak)const;
                
                void updVelocity(const State& state, 
                                const RigidBodyNode *Parent, 
                                const Rotation& prF_C_F, 
                                const double *betak,
                                Vect3& w,
                                Vect3& v) const ;
                
                void updRelVelocity(const State& state, Vect3& w, Vect3& v) const;
                
                int getJointDOF() const;	
                
                void calcQDot(State& state)const;
                
                void calc_H_x_UDot(const State& state, double *h_x_udot) const;
                
                void calcAKT(const RigidBodyNode *Parent, 
                        const RigidBodyNode *Body,
                        const int& ChildID,
                        const double *betak,
                        State& state,
                        Vect6& sAkt) const;
                
                void sweepForward(const RigidBodyNode *Parent,
                                const int& ChildID,
                                const Vect6& sAkt,
                                const Rotation& FCF,
                                const ABAData *aba,
                                const double *betak,
                                EOMData *eom,
                                State& state
                                );
                                
                void sweepBackwards(const bool& isLeaf, 
                                const int& ChildID,
                                const Vect6& sAkt,
                                const Rotation& FCF,					    
                                const EOMData *eom,
                                const double *betak,
                                RigidBodyNode *Parent,
                                ABAData *aba) const;
}; // class Cylinder

inline void Cylinder::getHColumn(const int& uindex, 
                                double *hptr)const{
        #ifdef RSIM_SAFE
        Utils::rsim_assert(uindex==0 || uindex==1,
                        "Joint::Cylinder::getHColumn(), uindex must be either 0 or 1");        
        #endif
        
        hptr[0] = 0.0;  hptr[1] = 0.0; 
        hptr[3] = 0.0;  hptr[4] = 0.0; 
        if(uindex){
                hptr[5]=1.0;
                hptr[2]=0.0;
        }
        else{
                hptr[2]=1.0;
                hptr[5]=0.0;
        }
}

} // namespace RSIM

#endif